/**
 * @file ros_can_driver.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief LCCAN 官方 SDK 封装文件，使其在 ROS 下更容易被使用
 * @version v2.1
 * @date 2020-06-25
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "ros_can_driver.h"

#include "ros/ros.h"

DWORD g_devicehandle;   //LCCAN Handle

void vcu_frame_data_format_out(void)
{
  BYTE vcu_frame_data_test[8] = {0xF9, 0x00, 0xFF, 0xFF, 0xFC, 0xFF, 0xF0, 0x00};
  VCU_FRAME_DATA * VCU_FRAME_DATA_TEST_P = (VCU_FRAME_DATA *)vcu_frame_data_test;
  
  ROS_INFO("VCU_FRAME_DATA size is %ld", sizeof(VCU_FRAME_DATA));
  
  ROS_INFO(  "\nVCU_GEAR_REQ:\t[0x%x]\nVCU_NULL_1:\t[0x%x]\nVCU_BREAK_REQ:\t[0x%x]\nVCU_STR_REQ:\t[0x%x]\nVCU_PED_REQ:\t[0x%x]\nVCU_MODE:\t[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_GEAR_REQ,
              VCU_FRAME_DATA_TEST_P->VCU_NULL_1,
              VCU_FRAME_DATA_TEST_P->VCU_BREAK_REQ,
              VCU_FRAME_DATA_TEST_P->VCU_STR_REQ,
              VCU_FRAME_DATA_TEST_P->VCU_PED_REQ,
              VCU_FRAME_DATA_TEST_P->VCU_MODE);
  ROS_INFO(  "\nVCU_NULL_2:\t[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_NULL_2);
  ROS_INFO(  "\nVCU_PED_VALUE:\t[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_PED_VALUE);
  ROS_INFO(  "\nVCU_STR_ANGLE_L:[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_STR_ANGLE_L);
  ROS_INFO(  "\nVCU_NULL_3:\t[0x%x]\nVCU_STR_ANGLE_H:[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_NULL_3,VCU_FRAME_DATA_TEST_P->VCU_STR_ANGLE_L);
  ROS_INFO(  "\nVCU_BREAK_VALUE:[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_BREAK_VALUE);
  ROS_INFO(  "\nVCU_NULL_4:\t[0x%x]\nVCU_GEAR_VALUE:\t[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_NULL_4,VCU_FRAME_DATA_TEST_P->VCU_GEAR_VALUE);
  ROS_INFO(  "\nVCU_NULL_5:\t[0x%x]\n",
              VCU_FRAME_DATA_TEST_P->VCU_NULL_5);
}

void vcu_frame_data_format_out(CAN_DataFrame *param)
{
  ROS_INFO("CAN_frame_data is as follow :");
  ROS_INFO("uTimeFlag is \t%d",param->uTimeFlag);
  ROS_INFO("nSendType is \t%d",param->nSendType);
  ROS_INFO("bRemoteFlag is \t%d",param->bRemoteFlag);
  ROS_INFO("bExternFlag is \t%d",param->bExternFlag);
  ROS_INFO("nDataLen is \t%d",param->nDataLen);
  ROS_INFO("uID is \t\t0x%x",param->uID);

  VCU_FRAME_DATA VCU_FRAME_DATA;
  memcpy(&VCU_FRAME_DATA,&(param->arryData), sizeof(VCU_FRAME_DATA));
  ROS_INFO("DATA is as follow :");
  ROS_INFO(  "\nVCU_GEAR_REQ:\t[0x%x]\nVCU_NULL_1:\t[0x%x]\nVCU_BREAK_REQ:\t[0x%x]\nVCU_STR_REQ:\t[0x%x]\nVCU_PED_REQ:\t[0x%x]\nVCU_MODE:\t[0x%x]\n",
              VCU_FRAME_DATA.VCU_GEAR_REQ,
              VCU_FRAME_DATA.VCU_NULL_1,
              VCU_FRAME_DATA.VCU_BREAK_REQ,
              VCU_FRAME_DATA.VCU_STR_REQ,
              VCU_FRAME_DATA.VCU_PED_REQ,
              VCU_FRAME_DATA.VCU_MODE);
  ROS_INFO(  "\nVCU_NULL_2:\t[0x%x]\n",
              VCU_FRAME_DATA.VCU_NULL_2);
  ROS_INFO(  "\nVCU_PED_VALUE:\t[0x%x]\n",
              VCU_FRAME_DATA.VCU_PED_VALUE);
  ROS_INFO(  "\nVCU_STR_ANGLE_H:[0x%x]\n",
              VCU_FRAME_DATA.VCU_STR_ANGLE_H);
  ROS_INFO(  "\nVCU_NULL_3:\t[0x%x]\nVCU_STR_ANGLE_L:[0x%x]\n",
              VCU_FRAME_DATA.VCU_NULL_3,VCU_FRAME_DATA.VCU_STR_ANGLE_L);
  ROS_INFO(  "\nVCU_BREAK_VALUE:[0x%x]\n",
              VCU_FRAME_DATA.VCU_BREAK_VALUE);
  ROS_INFO(  "\nVCU_NULL_4:\t[0x%x]\nVCU_GEAR_VALUE:\t[0x%x]\n",
              VCU_FRAME_DATA.VCU_NULL_4,VCU_FRAME_DATA.VCU_GEAR_VALUE);
  ROS_INFO(  "\nVCU_NULL_5:\t[0x%x]\n",
              VCU_FRAME_DATA.VCU_NULL_5);
}

DWORD LCCANInit(void)
{
  // 打开设备
  if ( (g_devicehandle = CAN_DeviceOpen(ACUSB_132B, DEF_DEV_INDEX, 0)) == 0 )
  {
    ROS_INFO("open deivce error\n");
    return 0;
  }

  CAN_DeviceInformation DevInfo;
  if ( CAN_GetDeviceInfo(g_devicehandle, &DevInfo) != CAN_RESULT_OK )
  {
    ROS_INFO("GetDeviceInfo error\n");
    return 0;
  }
  ROS_INFO("\t--%s--\n", DevInfo.szDescription);
  ROS_INFO("\tSN:%s\n", DevInfo.szSerialNumber);
  ROS_INFO("\tCAN 通道数:%d\n", DevInfo.bChannelNumber);
  ROS_INFO("\t硬件  版本:%x\n", DevInfo.uHardWareVersion);
  ROS_INFO("\t固件  版本:%x\n\n", DevInfo.uFirmWareVersion);
  ROS_INFO("\t驱动  版本:%x\n", DevInfo.uDriverVersion);
  ROS_INFO("\t接口库版本:%x\n", DevInfo.uInterfaceVersion);

  return 1;
}

DWORD LCCANChanleStart(void)
{
  // 启动CAN通道
  CAN_InitConfig config;
  config.dwAccCode = 0;
  config.dwAccMask = 0xffffffff;
  config.nFilter  = 0;        // 滤波方式(0表示未设置滤波功能,1表示双滤波,2表示单滤波)
  config.bMode    = 0;        // 工作模式(0表示正常模式,1表示只听模式)
  config.nBtrType = 1;        // 位定时参数模式(1表示SJA1000,0表示LPC21XX)
  config.dwBtr[0] = 0x00;     // BTR0   0014 -1M 0016-800K 001C-500K 011C-250K 031C-12K 041C-100K 091C-50K 181C-20K 311C-10K BFFF-5K
  config.dwBtr[1] = 0x1c;     // BTR1
  config.dwBtr[2] = 0;
  config.dwBtr[3] = 0;

  if ( CAN_ChannelStart(g_devicehandle, 0, &config) != CAN_RESULT_OK )
  {
    ROS_INFO("Start CAN 0 error!\n");
    return 0;
  }
  else
  {
    ROS_INFO("Start CAN 0 success!\n");
  }
  
  if ( CAN_ChannelStart(g_devicehandle, 1, &config) != CAN_RESULT_OK )
  {
    ROS_INFO("Start CAN 1 error!\n");
    return 0;
  }
  else{
    ROS_INFO("Start CAN 1 success!\n");
  }

  return 1;
}

void Send(CAN_DataFrame *param)
{
  uint8_t reclen = 0x3c;
  uint8_t channel = param->uID;
  param->uID = DEF_SEND_ID;
  
  // ROS_INFO("CAN Channel ID is %d", channel);
  // ROS_INFO("CAN Frame ID is [0x%x]", param->uID);
  // for(int i = 0; i < 8; ++i)
  // {
  //     ROS_INFO("msg.arrdata[%d] is [0x%x]", i, param->arryData[i]);
  // }
  // vcu_frame_data_format_out(param);
  reclen = CAN_ChannelSend(g_devicehandle, channel, param, 1);
  // ROS_INFO("reclen is %d", reclen);
}
